/**
 * \file Quatf.hpp
 * Header for Quaternions
 */
#ifndef _SIUT_SIMD_STANDARD_QUATF_HPP_
#define _SIUT_SIMD_STANDARD_QUATF_HPP_

#include <cmath>


#ifdef NVCC
#define CUDAHD __device__ __host__
#else
#define CUDAHD
#include "Vec3f.hpp"
#ifdef DEBUG_QUAT
#include <iostream>
#include <sstream>
#include <string>
#include <stdexcept>
#endif
#endif

namespace siut {
  namespace simd {
    /** Struct for quaternions, should be compatible with CUDA.
     * Has functions for most Quat to Quat operations, as well as some Quat to float.
     * No testing enabled by default, but can throw runtime_error for division by 0 and index out of range by defining DEBUG_QUAT.
     */
    struct Quatf
    {
#ifdef NVCC
      /**
       * For CUDA, using built-in float4 structure.
       */
      float4 q_;
#else
      /** If not for cuda using float[4] array.
       */
      float q_[4];
#endif
	
      /*-----------------------------------------------------------------------*/
      /*   Quatf                                                               */
      /*-----------------------------------------------------------------------*/
    
      /** Default constructor, works with both CUDA and cpu.
       * Sets 0-rotation of 1, 0, 0, 0.
       */
      CUDAHD Quatf()
      {
	q_[0] = 1; q_[1] = q_[2] = q_[3] = 0;
      }


#ifndef NVCC
      /** Non-CUDA constructor, creates quaternion from the floats given with W being rotation.*/
      Quatf(float w, float x, float y, float z)
      {
	q_[0] = w;
	q_[1] = x;
	q_[2] = y;
	q_[3] = z;
      }
      /** Non-CUDA constructor, creates quaternion from the float-array given with tab[0] being rotation.*/
      Quatf(float tab[4])
      {
	q_[0] = tab[0];
	q_[1] = tab[1];
	q_[2] = tab[2];
	q_[3] = tab[3];
      }

      /** Non-CUDA constructor, creates quaternion from the float-pointer given with pf[0] being rotation.*/
      Quatf(const float *pf)
      {
	q_[0] = pf[0];
	q_[1] = pf[1];
	q_[2] = pf[2];
	q_[3] = pf[3];
      }
      /** Non-CUDA constructor, creates quaternion from the given quaternion.*/
      Quatf(const Quatf &q)
      {
	q_[0] = q.q_[0];
	q_[1] = q.q_[1];
	q_[2] = q.q_[2];
	q_[3] = q.q_[3];
      }       

      /** Non-CUDA constructor, creates quaternion from the scalar, which is rotation, and the vector, for axis.*/
      Quatf(const float s, const Vec3f &v)
      {
	q_[0] = s;
	q_[1] = v.x();
	q_[2] = v.y();
	q_[3] = v.z();
      }
#endif

      /*-----------------------------------------------------------------------*/
      /* getVector() getScalar() setVector() setScalar()                       */
      /*-----------------------------------------------------------------------*/

      /** Returns the scalar part of the quaternion. */
      inline CUDAHD float getScalar()
      {
	return q_[0];
      }

      /** Sets the scalar of the quaternion to f. */
      inline CUDAHD void setScalar(const float f)
      {
	q_[0] = f;
      }


#ifdef NVCC
      /** Returns the vector part of the quaternion as a float3. */
      inline CUDAHD float3 getVector()
#else
      /** Returns the vector part of the quaternion as a Vec3f. */
	inline Vec3f getVector()
#endif
      {
	return Vec3f(q_[1], q_[2], q_[3]);
      }


#ifdef NVCC
      /** Sets the vector part of the quaternion from a float3. */
      inline CUDAHD void setVector(const float3 &v)
#else
      /** Sets the vector part of the quaternion from a Vec3f. */
	inline void setVector(const Vec3f &v)
#endif
      {
	q_[1] = v[0];
	q_[2] = v[1];
	q_[3] = v[2]; 
	
      }

      /** Sets the vector part of the quaternion from the given floats. */
      inline CUDAHD void setVector(float b, float c, float d)
      {
	q_[1] = b;
	q_[2] = c;
	q_[3] = d;
      }


      /*-----------------------------------------------------------------------*/
      /*    functions w() x() y() z() ()                                    */
      /*-----------------------------------------------------------------------*/
      /** Returns the w component of the quaternion, const version. */
      inline CUDAHD const float& w() const
      {
	return q_[0];
      }
    
      /** Returns the x component of the quaternion, const version. */
      inline CUDAHD const float& x() const
      {
	return q_[1];
      }

      /** Returns the y component of the quaternion, const version. */
      inline CUDAHD const float& y() const
      {
	return q_[2];
      }

      /** Returns the z component of the quaternion, const version. */
      inline CUDAHD const float& z() const
      {
	return q_[3];
      }

      /** Returns the w component of the quaternion, non-const version. */
      inline CUDAHD float& w()
      {
	return q_[0];
      }
    
      /** Returns the x component of the quaternion, non-const version. */
      inline CUDAHD float& x()
      {
	return q_[1];
      }

      /** Returns the y component of the quaternion, non-const version. */
      inline CUDAHD float& y()
      {
	return q_[2];
      }

      /** Returns the z component of the quaternion, non-const version. */
      inline CUDAHD float& z()
      {
	return q_[3];
      }



	
      /*-----------------------------------------------------------------------*/
      /*    operators + * += *= / /= set()                                     */
      /*-----------------------------------------------------------------------*/     
      /** Add quaternion with another, saving result in left quaternion. */
      inline CUDAHD Quatf& operator += (const Quatf &a)
      {
	q_[0] += a.q_[0];
	q_[1] += a.q_[1];
	q_[2] += a.q_[2];
	q_[3] += a.q_[3];

	return *this;
      }

      /** Multiply quaternion with another, saving result in left quaternion.
       * Multiplication following this formula:
       * q1q2 = s1s2 - v1 . v2 + s1v2 + s2v1 + v1 x v2;
       * s is scalar part, v is vector part
       * '.' is dot product and 'x' is cross product.
       */
      inline CUDAHD Quatf& operator *= (const Quatf &i)
      {
	float b_[4] = {q_[0], q_[1], q_[2], q_[3]};
      
	q_[0] = b_[0] * i.q_[0] - (b_[1] * i.q_[1] + b_[2] * i.q_[2] + b_[3]*i.q_[3]);
	q_[1] = b_[0] * i.q_[1] + i.q_[0] * b_[1] + b_[2] * i.q_[3] - i.q_[2] * b_[3];
	q_[2] = b_[0] * i.q_[2] + i.q_[0] * b_[2] + b_[3] * i.q_[1] - i.q_[3] * b_[1];
	q_[3] = b_[0] * i.q_[3] + i.q_[0] * b_[3] + b_[1] * i.q_[2] - i.q_[1] * b_[2];
      
	return *this;
      }
    
      /** Divide quaternion component wise with the float, saving result in left quaternion. */
      inline CUDAHD Quatf& operator /= (const float f)
      {
	q_[0] /= f;
	q_[1] /= f;
	q_[2] /= f;
	q_[3] /= f;
	  
	return *this;
      }

      /** Add two quaternions together, returning a new quaternion with the result.
       * Note: Not using new Quatf()! */
      inline CUDAHD Quatf operator + (const Quatf &q)
      {
	Quatf res(q_);
	res += q;
	return res;
      }
      /** Multiply two quaternions together, returning a new quaternion with the result. 
       * Note: Not using new Quatf()!
       * Same formula as operator *=().
       */
      inline CUDAHD Quatf operator * (const Quatf &q)
      {
	Quatf res(q_);
	res *= q;
	return res;
      }

      /** Divide a quaternion by a float, returning a new quaternion with the result. 
       * Note: Not using new Quatf()!
       */
      inline CUDAHD Quatf operator / (const float f)
      {
	Quatf res(q_);
	res /= f;
	return res;
      }

      /** Returns component i of the Vector. 
       * Will do a check if index is out of bounds if DEBUG_QUAT is defined.
       */
      inline float & operator [] (int i)
      {
#ifdef DEBUG_QUAT
	if(i > 3)
	  {
	    std::stringstream s;
	    s << "index larger than 3 in " << __FILE__ << " at " << (int) __LINE__ << std::endl;
	    throw std::runtime_error(s.str() );
	  }
#endif
	return q_[i];
      }

      /** Returns component i of the Vector, const version. 
       * Will do a check if index is out of bounds if DEBUG_QUAT is defined.
       */
      inline const float & operator [] (int i) const
      {
#ifdef DEBUG_QUAT
	if(i > 3)
	  {
	    std::stringstream s;
	    s << "index larger than 3 in " << __FILE__ << " at " << (int) __LINE__ << std::endl;
	    throw std::runtime_error(s.str() );
	  }
#endif
	return q_[i];
      }

     


    /*-----------------------------------------------------------------------*/
    /*    functions get() set()                                              */
    /*-----------------------------------------------------------------------*/    
      
      /** Returns component i of the Vector. 
       * Will do a check if index is out of bounds if DEBUG_QUAT is defined.
       */
      inline CUDAHD float & get(int i) 
      {
#ifdef DEBUG_QUAT
	if(i > 3)
	  {
	    std::stringstream s;
	    s << "index larger than 3 in " << __FILE__ << " at " << (int) __LINE__ << std::endl;
	    throw std::runtime_error(s.str() );
	  }
#endif	
	return q_[i]; 
      }
      
      /**Sets the ith component to f.
       * Will do a check if index is out of bounds if DEBUG_QUAT is defined.
       */
      inline CUDAHD void set(const int i, const float f)
      {
#ifdef DEBUG_QUAT
	if(i > 3)
	  {
	    std::stringstream s;
	    s << "index larger than 3 in " << __FILE__ << " at " << (int) __LINE__ << std::endl;
	    throw std::runtime_error(s.str() );
	  }
#endif	
	q_[i] = f;
      }


   };//end of struct


    /*-----------------------------------------------------------------------*/
    /*    functions conjugate() norm() operator ~                            */
    /*-----------------------------------------------------------------------*/    
    /** Conjugate/Negates the axis of the rotation. */
    inline CUDAHD Quatf operator~ (const Quatf &a)
    {
      return Quatf(a.q_[0], -a.q_[1], -a.q_[2], -a.q_[3]);
    }
    
    /** Return the norm/length of the quaternion.
     * sqrt(w^2 + x^2 + y^2 + z^2)
     */
    inline CUDAHD float norm(const Quatf &a)
    {
      return std::sqrt(pow(a.q_[0],2)+pow(a.q_[1],2)+pow(a.q_[2],2)+pow(a.q_[3],2));
    }

    /** Conjugate/Negates the axis of the rotation. */
    inline CUDAHD Quatf conjugate (const Quatf &a)
    {
      return Quatf(a.q_[0], -a.q_[1], -a.q_[2], -a.q_[3]);
    }
      
    /** Normalizes the quaternion. */
    inline CUDAHD Quatf normalize(const Quatf &a)
    {
      float f = norm(a);
      Quatf ret(a);
      ret /= f;
      return ret;      
    }

	inline CUDAHD void addScaledVector(Quatf &a, const Vec3f& vector, const float scale)
	{
		Quatf q(0, 
			vector.x() * scale,
			vector.y() * scale,
			vector.z() * scale);
		q *= a;
		a.w() += q.w() * 0.5f;
		a.x() += q.x() * 0.5f;
		a.y() += q.y() * 0.5f;
		a.z() += q.z() * 0.5f;		
	}

	inline CUDAHD void FromAxis(Quatf &a, const Vec3f &v, float angle)
	{
		float sinAngle;
		angle *= 0.5f;
		Vec3f vn(v);
		normalize(vn);

		sinAngle = sin(angle);

		a.x() = (vn.x() * sinAngle);
		a.y() = (vn.y() * sinAngle);
		a.z() = (vn.z() * sinAngle);
		a.w() = cos(angle);
	}



    
  } //namespace simd
} //namespace siut
#endif
